from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    pkg_share = get_package_share_directory('xj_wheel')
    urdf_path = os.path.join(pkg_share, 'urdf', 'xj_wheel.urdf')

    use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='true')
    gui = DeclareLaunchArgument('gui', default_value='true')

    # Publish robot_description from URDF file
    with open(urdf_path, 'r') as f:
        robot_description_content = f.read()

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[{
            'use_sim_time': LaunchConfiguration('use_sim_time'),
            'robot_description': robot_description_content,
        }],
        output='screen'
    )

    # Include official Gazebo (classic) launcher from gazebo_ros
    gazebo_ros_share = get_package_share_directory('gazebo_ros')
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(gazebo_ros_share, 'launch', 'gazebo.launch.py')
        ),
        launch_arguments={
            'gui': LaunchConfiguration('gui')
        }.items()
    )

    # Spawn entity from robot_description
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic', 'robot_description', '-entity', 'xj_wheel', '-x', '0', '-y', '0', '-z', '0.2'],
        output='screen'
    )

    return LaunchDescription([
        use_sim_time,
        gui,
        gazebo_launch,
        robot_state_publisher,
        spawn_entity,
    ])


